﻿using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Windows.Forms;
using componentXtra;
using System.Drawing.Drawing2D;

namespace Simulador_Quadrotor
{
    public partial class Simulador2DForm : Form
    {
        #region Atributos
        ConfiguracionPurebaPID configuracionPurebaPID = null;
        Controlador controlador = null;
        Dinamica dinamica = null;
        double[] rot = null;
        Estado estadoInicial = null;
        Estado estadoActual = null;
        Estado estadoFinal = null;
        double tp = 0;
        double tiempoTotal = 0;
        #endregion

        public Simulador2DForm()
        {
            InitializeComponent();
        }

        private void buttonControladorPID_Click(object sender, EventArgs e)
        {

            this.openFileDialog1.AddExtension = true;
            this.openFileDialog1.DefaultExt = "Configuracion PID (*.cfPID)|*.cfPID|Todos los archivos (*.*)|*.*";
            this.openFileDialog1.Filter = "Configuracion PID (*.cfPID)|*.cfPID|Todos los archivos (*.*)|*.*";
            this.openFileDialog1.Title = "Cargar configuracion de PID";
            if (this.openFileDialog1.ShowDialog() == DialogResult.OK)
            {
                string path = this.openFileDialog1.FileName;
                this.textBoxRutaPID.Text = path;
                Serializador serializador = new Serializador(path);
                configuracionPurebaPID = serializador.DeserializarXML(typeof(ConfiguracionPurebaPID)) as ConfiguracionPurebaPID;
                this.tp = configuracionPurebaPID.TPaso;
            }
        }

        private void timer1_Tick(object sender, EventArgs e)
        {
            this.estadoFinal = new Estado(0, 0, 0, 0, 0, 0, (double)this.numericUpDownPosicionX.Value, (double)this.numericUpDownPosicionY.Value, (double)this.numericUpDownAltura.Value, 0, 0, 0);
            //vemso cuando tenemos que descender
            double errorX = this.estadoFinal.X - this.estadoActual.X;
            double errorY = this.estadoFinal.Y - this.estadoActual.Y;
            double errorAltura = this.estadoFinal.Z - this.estadoActual.Z;
            //Si el error es pequeño empezamos el descenso
            if (errorX < 1 && errorY < 1)
            {
                this.estadoFinal.Z = 0;
                if (errorAltura < 0.001 && this.estadoActual.Z == 0)
                {

                    this.timer1.Enabled = false;
                    MessageBox.Show(this, "Posicon final--> X=" + estadoActual.X + "  Y=" + estadoActual.Y, "Fin", MessageBoxButtons.OK, MessageBoxIcon.Information);
                }
            }

            //calculamos el estado
            this.estadoActual = this.dinamica.IntegraPaso(ref this.tiempoTotal, this.tp, this.rot);
            //controlamos los rotores
            rot = controlador.Controla(this.estadoActual, this.estadoFinal);
            //pintamos el resultado
            this.PintaGrafica(this.tiempoTotal, this.estadoActual.Z, xyGraph2);
            this.PintaGrafica(this.estadoActual.X, this.estadoActual.Y, GraficaPlanoXY);
            //actualizamos datos
            this.textBoxPitch.Text = this.estadoActual.theta.ToString();
            this.textBoxRoll.Text = this.estadoActual.phi.ToString();
            this.textBoxYaw.Text = this.estadoActual.theta.ToString();
            this.textBoxTiempo.Text = this.tiempoTotal.ToString();
            this.textBoxRot1.Text = rot[0].ToString();
            this.textBoxRot2.Text = rot[1].ToString();
            this.textBoxRot3.Text = rot[2].ToString();
            this.textBoxRot4.Text = rot[3].ToString();
            //incrementamos el tiempo
            this.tiempoTotal += this.tp;


        }

        private void button1Comenzar_Click(object sender, EventArgs e)
        {

            this.GraficaPlanoXY.ClearGraphs();
            this.xyGraph2.ClearGraphs();
            rot = new double[] { 0, 0, 0, 0 };
            this.estadoInicial = new Estado(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0);
            this.estadoActual = new Estado(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0);
            this.estadoFinal = new Estado(0, 0, 0, 0, 0, 0, (double)this.numericUpDownPosicionX.Value, (double)this.numericUpDownPosicionY.Value, (double)this.numericUpDownAltura.Value, 0, 0, 0);
            controlador = new PIDControl(configuracionPurebaPID);
            this.timer1.Interval = int.Parse((configuracionPurebaPID.TPaso * 1000).ToString());
            this.timer1.Enabled = true;
            this.dinamica = new Dinamica(rot, estadoInicial);



        }
        private void PintaGrafica(double tiempo, double valor, XYGraph grafica)
        {

            grafica.AddGraph("", DashStyle.Solid, Color.Yellow, 0, false);
            grafica.AddValue(0, (float)tiempo, (float)valor);
            grafica.DrawAll();
        }

        private void buttonStop_Click(object sender, EventArgs e)
        {
            this.timer1.Enabled = false;
        }
    }
}
